#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTServo,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     tLimits,        sensorI2CCustom9V)
#pragma config(Sensor, S4,     tIRBeacon,      sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  motorA,          mTow,          tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorB,          mLeftTow,      tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          mRightTow,     tmotorNXT, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     dLeft1,        tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     dLeft2,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     dRight1,       tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     dRight2,       tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C2_1,     aBallTip,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     aUExtend,      tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C3_1,     aBallPickup,   tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C3_2,     aLExtend,      tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S2_C1_1,    sBallDrop,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    sTowGoal,             tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    sIRSeeker,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "FTC_Toolkit.h"

void initializeRobot()
{
  motor[dLeft1] = motor[dLeft2] = motor[dRight1] = motor[dRight2] = 0;
  motor[aBallTip] = motor[aUExtend] = motor[aBallPickup] = motor[aLExtend] = 0;
  servo[sBallDrop] = 255;
  servo[sIRSeeker] = 100;

  return;
}

// Main Task

task main()
{

  initializeRobot();

  waitForStart();   // wait for start of tele-op phase

  while(true)
  {
	  getJoystickSettings(joystick);

	  TeleopDriveSystem(joystick.joy1_y1, joystick.joy1_x2);
	  BallPickup(joy2Btn(5), joy2Btn(7));
	  BallPickupTip(joy2Btn(6), joy2Btn(8));
	  BallDrop(joy2Btn(2));
	  TowTeleop(joy1Btn(4), joy1Btn(2));

	  if


	  LowExtension(joystick.joy2_y1);
	  HighExtension(joystick.joy2_y2);

	  abortTimeslice();
  }
}
